#include "CSkeleton.h"
#include "utils.h"
#include "GL\glut.h"
#include "CVector.h"
#include "Vector3df.h"
#include "CVolume.h"

using namespace PinguX::Math;
//, define value 0x0501 , must change or undefine

CSkeleton::CSkeleton()
{
	maxPts = 20; //For now...
	smooth = 0.5;
	size = 0.5;
	segs = 1.0;
	tension = 0.5;
	change = 1.0;
	volume = 0.5;
	duration = 0.5;
	octave = 5.;
	prevLocValid = false;
	progMode = true;

	testCount = 0;

	smW = 0.1; //Set the weights... TODO: adjust this
	szW = 0.1;
	sgW = 0.1;
	tnW = 0.1;
	chW = 0.1;
	vlW = 0.1;
	drW = 0.1;
}


CSkeleton::~CSkeleton()
{
	RH.clear();
	LH.clear();
	H.clear();
	T.clear();
	vol_main(0.3);//Make it at least somewhat quiet... heh...
}


void CSkeleton::addpt(XnSkeletonJoint j, XnSkeletonJointPosition p)
{
	//printf("%d: (%f,%f,%f) [%f]\n", uID, Head.position.X, Head.position.Y, Head.position.Z, Head.fConfidence);


	//IMPORTANT: TODO: Is this position in real world coords? Or in window? check for conversion function



	//Add the points, trim the vector if needed
	switch (j)
	{
	case XN_SKEL_RIGHT_HAND: {
		RH.push_back(p);
		pt newpt;
		pts.push_back(newpt);
		if ((int)RH.size() > maxPts) { RH.erase(RH.begin()); pts.erase(pts.begin()); }
		break; }
	case XN_SKEL_LEFT_HAND:
		LH.push_back(p);
		if ((int)LH.size() > maxPts) LH.erase(LH.begin());
		break;
	case XN_SKEL_HEAD:
		H.push_back(p);
		if ((int)H.size() > maxPts) H.erase(H.begin());
		break;
	case XN_SKEL_TORSO:
		T.push_back(p);
		if ((int)T.size() > maxPts) T.erase(T.begin());
		break;
	default:
		//Ignore
		break;
	}
}


std::vector<float> CSkeleton::EvalPts()
{
	//Go through each of RH, LH, H, and T point vectors, add to the various attrs, return in single vector
	//attrs: smooth, size, segs, tension, change


	//Sliding window method
	int win = 5;
	Vector3df prev; //keep track of previous vector
	double prevAngle = 0.;

	int mx = (int)RH.size(); //Max # of pts
	int e=0;
	for (; e<mx-1; e++)
	{
		if (!pts[e].evl) //Find the earliest non-evaled pt
			break;
	}

	float bottomY = 0, topY = winY; //Keep track of the highest and lowest Y's, for size/volume... Same for X? Z (vol)?
	float leftX = winX, rightX = 0;
	
	float totY = 0;
	int count = 0;

	bool isClk = true; //Keep track of most recent clockwise/counterclockwise
	for (int i=max(e-4,0); i<mx-1; i++) //Don't care about the last point by itself
	{
		pts[i].evl = true;

		std::vector< Vector3df > vecs;
		XnVector3D p1 = RH[i].position;
		if (p1.X <= 75. && p1.X >= 25. && p1.Y <= 375. && p1.Y >= 300.)
			progMode = true;

		if (p1.X <= 75. && p1.X >= 25. && p1.Y <= 575. && p1.Y >= 500.)
			progMode = false;
			
			
		totY += p1.Y;
		count++;
		XnVector3D p2 = RH[i+1].position; //Will be valid, i stops at size-1


		//-------------------------SMOOTHNESS------------------------------
		//For smoothness calculations:
		if (i > 0)
		{
			//We know that i-1, i, and i+1 are all valid. Find the angle they form.
			float ag = angle(p1, RH[i-1].position, p2); //Angle in radians. Make sure it's between 0 and pi
			
			float pi = 3.14159265;
			while (ag > 2*pi) ag -= 2*pi; //Get it into a normal circle
			while (ag < 0.) ag += 2*pi;

			if (ag > pi) ag = pi - (ag - pi); //just want the angles that look jagged

			float newAg = scale(0.0, ag, pi);
			adjust(smooth, newAg); //Small angles = small smoothness (more jagged)
		}


		
		if (p1.Y < topY) topY = p1.Y; //The Y that's closest to the top is the lowest Y value... watch the names
		if (p1.Y > bottomY) bottomY = p1.Y;
		if (p1.X < leftX) leftX = p1.X;
		if (p1.X > rightX) rightX = p1.X;

		//Vector3df first(p1.X, p1.Y, 0.); //Ignore z?
		//Vector3df second(p2.X, p2.Y, 0.);

		//TODO: Are we doing too many evaluations? Might be fixed with pt.evl...
		Vector3df cur(p2.X-p1.X, p2.Y-p1.Y, 0.);
		if (i!=0)
		{
			//If not the first point...
			double n1 = sqrt(prev.dot(prev)), n2 = sqrt(cur.dot(cur));
			double angle = acos(prev.dot(cur)/(n1*n2)); //angle in radians
			double ptDist = dist(p1, p2);

			double angDif = angle-prevAngle; 
			if (angle > prevAngle && ptDist >= 10.)
			{
				//Switch to Counter-clockwise
				if (pts[i].clk)
				{
					//If previously clockwise, there's a change in direction, mark it as end of segment
					pts[i].end = true;
					pts[i].clk = false;
					isClk = false;
				}
				//Otherwise, just continue
			}
			else if (angle == prevAngle || ptDist < 25.)
			{
				//Not sure how often this will happen, just keep whatever direction was previous... anything else?
				//This is so that really small movements don't mess up everything,
				//   because there is some noise in mov't detection
				pts[i].clk = isClk;
			}
			else
			{
				//Switch to Clockwise
				if (!pts[i].clk)
				{
					//If previously counter-clockwise, there's a change in direction, add to line segment count
					pts[i].end = true;
					pts[i].clk = true;
					isClk = true;
				}
			}

			//After done, let's reset prev and prevAngle
			prev = cur;
			prevAngle = angle;

			



			//-------------------------SIZE------------------------------
			//Now, see how far away i is from nearby points
			int left = max(i-2, 0); //Make sure they're valid
			int right = min(i+2, mx-1);

			//If d is large, that means this is more jagged... if not, it's smoother
			float d = lineDist(RH[i].position, RH[left].position, RH[right].position);

			float maxDist = 250.f; //For use in scaling...

			//if (d>maxDist) 
				//printf("d is too large!\n");
			
			d = min(d, maxDist);

			if (d < 0)
			{
				d = 0;
				//And we have a problem...
			}

			float newD = scale(0.0, d, maxDist);
			adjust(size, newD); //Why are we changing size here...oh





			
			//-------------------------TENSION------------------------------
			if (i < (int)LH.size()-1)
			{
				//Check the corresponding point in the left hand, add to tension
				float handDist = dist(LH[i].position, RH[i].position);
				handDist = minmax(0., handDist, winX/3);

				float prop = newScale(0.0, 1.0, 0., winX/3, handDist);
				prop = 1.-prop; //Reverses it so 0 = low tension
				adjust(tension, prop);
				if (tension > 1. || tension < 0.)
				{
					int a = 0; 
					//Another problem...
				}
			}







		}//end if (i != 0)


	}//end point traversal: RH








	//Now left hand
	for (int i=max(e-4,0); i<mx-1; i++) //Don't care about the last point by itself
	{
		pts[i].evl = true;

		std::vector< Vector3df > vecs;
		XnVector3D p1 = LH[i].position;
		//totY += p1.Y;
		//count++;
		XnVector3D p2 = LH[i+1].position; //Will be valid, i stops at size-1

		
		if (p1.X <= 75. && p1.X >= 25. && p1.Y <= 375. && p1.Y >= 300.)
			progMode = true;

		if (p1.X <= 75. && p1.X >= 25. && p1.Y <= 575. && p1.Y >= 500.)
			progMode = false;


		//-------------------------SMOOTHNESS------------------------------
		//For smoothness calculations:
		if (i > 0)
		{
			//We know that i-1, i, and i+1 are all valid. Find the angle they form.
			float ag = angle(p1, LH[i-1].position, p2); //Angle in radians. Make sure it's between 0 and pi
			
			float pi = 3.14159265;
			while (ag > 2*pi) ag -= 2*pi; //Get it into a normal circle
			while (ag < 0.) ag += 2*pi;

			if (ag > pi) ag = pi - (ag - pi); //just want the angles that look jagged

			float newAg = scale(0.0, ag, pi);
			adjust(smooth, newAg); //Small angles = small smoothness (more jagged)
		}


		
		//if (p1.Y < topY) topY = p1.Y; //The Y that's closest to the top is the lowest Y value... watch the names
		//if (p1.Y > bottomY) bottomY = p1.Y;
		//if (p1.X < leftX) leftX = p1.X;
		//if (p1.X > rightX) rightX = p1.X;

		//Vector3df first(p1.X, p1.Y, 0.); //Ignore z?
		//Vector3df second(p2.X, p2.Y, 0.);

		//TODO: Are we doing too many evaluations? Might be fixed with pt.evl...
		Vector3df cur(p2.X-p1.X, p2.Y-p1.Y, 0.);
		if (i!=0)
		{
			//If not the first point...
			double n1 = sqrt(prev.dot(prev)), n2 = sqrt(cur.dot(cur));
			double angle = acos(prev.dot(cur)/(n1*n2)); //angle in radians
			double ptDist = dist(p1, p2);

			double angDif = angle-prevAngle; 
			if (angle > prevAngle && ptDist >= 10.)
			{
				//Switch to Counter-clockwise
				if (pts[i].clk)
				{
					//If previously clockwise, there's a change in direction, mark it as end of segment
					pts[i].end = true;
					pts[i].clk = false;
					isClk = false;
				}
				//Otherwise, just continue
			}
			else if (angle == prevAngle || ptDist < 25.)
			{
				//Not sure how often this will happen, just keep whatever direction was previous... anything else?
				//This is so that really small movements don't mess up everything,
				//   because there is some noise in mov't detection
				pts[i].clk = isClk;
			}
			else
			{
				//Switch to Clockwise
				if (!pts[i].clk)
				{
					//If previously counter-clockwise, there's a change in direction, add to line segment count
					pts[i].end = true;
					pts[i].clk = true;
					isClk = true;
				}
			}

			//After done, let's reset prev and prevAngle
			prev = cur;
			prevAngle = angle;

			



			//-------------------------SIZE------------------------------
			//Now, see how far away i is from nearby points
			int left = max(i-2, 0); //Make sure they're valid
			int right = min(i+2, mx-1);

			//If d is large, that means this is more jagged... if not, it's smoother
			float d = lineDist(LH[i].position, LH[left].position, LH[right].position);

			float maxDist = 250.f; //For use in scaling...

			//if (d>maxDist) 
				//printf("d is too large!\n");
			
			d = min(d, maxDist);

			if (d < 0)
			{
				d = 0;
				//And we have a problem...
			}

			float newD = scale(0.0, d, maxDist);
			adjust(size, newD); //Why are we changing size here...oh





			
			//-------------------------TENSION------------------------------
			/*if (i < (int)LH.size()-1)
			{
				//Check the corresponding point in the left hand, add to tension
				float handDist = dist(LH[i].position, RH[i].position);
				handDist = minmax(0., handDist, winX/3);

				float prop = newScale(0.0, 1.0, 0., winX/3, handDist);
				prop = 1.-prop; //Reverses it so 0 = low tension
				adjust(tension, prop);
				if (tension > 1. || tension < 0.)
				{
					int a = 0; 
					//Another problem...
				}
			}*/







		}//end if (i != 0)


	}//end point traversal: LH



























	float totalAverageY = totY/count;
	octave = newScale(2., 7., 0., 10., (10 - (minmax(0.0, ((totalAverageY + 50) / (winY + 50)), 1.0)) * 10)); //Low = 0, high = 10

	//------------------DURATION & VOLUME---------------------
	//topY, bottomY, leftX, rightX all mark the largest corresponding values out of the RH points
	//If left/right are far apart, make the note duration longer
	//If topY, bottomY are far apart, make volume louder

	float maxDist = 200.; //Might need to have separate for x, y.
	float xDist = minmax(0., rightX - leftX, maxDist);//CHOMP
	float yDist = minmax(0., bottomY - topY, maxDist);
	
	float newDur = scale(0.0, xDist, maxDist);
	float newVol = scale(0.0, yDist, maxDist);

	adjust(volume, newVol);
	vol_main(newScale(0.2, 0.95, 0.0, 1.0, volume)); //And let that be the end of that. 
	adjust(duration, newDur);


	

	//--------------------------TORSO POSITION-------------------------------

	//Now evaluate torso for general body position (to influence change of chord)
	float allX = 0., allY = 0., allZ = 0., ct = 0;
	//Vector3df prevLoc;
	for (int t=0; t<(int)T.size(); t++)
	{
		if (T[t].fConfidence > minConf)
		{
			allX += T[t].position.X;
			allY += T[t].position.Y;
			allZ += T[t].position.Z;
			ct += 1;
		}

		/*if (!prevLocValid) {
			prevLoc.set(T[t].position.X, T[t].position.Y, T[t].position.Z);
			prevLocValid = true; }
		else {
			//Now that there's a previous location

			//Do we want to do previous locations? Why not set up just general areas for chords right now
			
		}*/
	}

	if (ct > 0) //If there's at least 1 point with good confidence, change chord
	{
		Vector3df newPos(allX/ct, allY/ct, allZ/ct);
		avgLoc = newPos;
		std::string toprint = "("+ftos(newPos.x())+","+ftos(newPos.y())+","+ftos(newPos.z())+")";

		glColor3d(1.,1.,1.);
		printw(200.,200.,0,toprint);
		change = newChord(newPos, winX, winY);
		if (T[0].fConfidence < minConf)
			change = 0.;
	}



	//-------------Segments-------------
	//Count how many end points were marked
	segs = 0;
	for (int i=0; i<(int)pts.size(); i++)
	{
		if (pts[i].end)
			segs++;
	}





	//---------------Finally, return the attrs---------------
	std::vector<float> attrs;
	attrs.push_back(smooth);	//0
	attrs.push_back(size);		//1
	attrs.push_back(segs);		//2
	attrs.push_back(tension);	//3
	attrs.push_back(change);	//4
	attrs.push_back(volume);	//5
	attrs.push_back(duration);	//6
	attrs.push_back(octave);	//7

	//testCount++;
	return attrs;
}


void CSkeleton::Draw()
{
	//Draw the trail of points that follow each of the joints
	//TODO: might be better to do something different in final version

	//Right now, testing by drawing a square around most recent point

	//Draw RH - purple

	glColor3d(0.58,0.,0.83);
	if ((int)RH.size() > 0)
		DrawRect(RH[RH.size()-1].position);
	glBegin(GL_LINES);
	for (int i=0; i < (int)RH.size(); i++)
	{
		if (RH[i].fConfidence >= minConf)
			glVertex2f(RH[i].position.X, RH[i].position.Y);
	}
	glEnd();

	//Draw LH - red
	glColor3d(1.,0.,0.);
	if ((int)LH.size() > 0)
		DrawRect(LH[LH.size()-1].position);
	glBegin(GL_LINES);
	for (int i=0; i < (int)LH.size(); i++)
	{
		if (LH[i].fConfidence >= minConf)
			glVertex2f(LH[i].position.X, LH[i].position.Y);
	}
	glEnd();

	/*//Draw H - blue
	glColor3d(0.,0.,1.);
	if ((int)H.size() > 0)
		DrawRect(H[H.size()-1].position);
	glBegin(GL_LINES);
	for (int i=0; i < (int)H.size(); i++)
	{
		if (H[i].fConfidence >= minConf)
			glVertex2f(H[i].position.X, H[i].position.Y);
	}
	glEnd();

	//Draw T - green
	glColor3d(0.,1.,0.);
	if ((int)T.size() > 0)
		DrawRect(T[T.size()-1].position);
	glBegin(GL_LINES);
	for (int i=0; i < (int)T.size(); i++)
	{
		if (T[i].fConfidence >= minConf)
			glVertex2f(T[i].position.X, T[i].position.Y);
	}
	glEnd();*/


	
	float ydif = 35.f;
	float xdif = 10.f;
	float wd = 190.f;
	float ht = 215.f;
	glEnable(GL_POLYGON_STIPPLE);
	glColor3d(.8,.8,.8);
	GLubyte halftone[] = {
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55,
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55,
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55,
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55,
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55, 
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55,
      0xAA, 0xAA, 0xAA, 0xAA, 0x55, 0x55, 0x55, 0x55};
	glPolygonStipple(halftone);
	glBegin(GL_QUADS);
		glVertex3d(xdif/2, 5.f, 0.f);
		glVertex3d(xdif/2, 5.f+ht, 0.f);
		glVertex3d(xdif/2+wd, 5.f+ht, 0.f);
		glVertex3d(xdif/2+wd, 5.f, 0.f);
	glEnd();
	glDisable(GL_POLYGON_STIPPLE);

	glLineWidth(5.);
	glBegin(GL_LINE_LOOP);
		glVertex3d(xdif/2, 5.f, 0.f);
		glVertex3d(xdif/2, 5.f+ht, 0.f);
		glVertex3d(xdif/2+wd, 5.f+ht, 0.f);
		glVertex3d(xdif/2+wd, 5.f, 0.f);
	glEnd();

	std::string smStr = "Smoothness: " + ftos(smooth);
	std::string szStr = "Size: " + ftos(size);
	std::string sgStr = "Segments: " + ftos(segs);
	std::string tnStr = "Tension: " + ftos(tension);
	std::string chStr = "Change: " + ftos(change);
	std::string ocStr = "Octave: " + ftos(octave);
	
	glColor3d(.3,.0,.8);
	printw(xdif, ydif, 0.f, smStr);
	printw(xdif, 2*ydif, 0.f, szStr);
	printw(xdif, 3*ydif, 0.f, sgStr);
	printw(xdif, 4*ydif, 0.f, tnStr);
	printw(xdif, 5*ydif, 0.f, chStr);
	printw(xdif, 6*ydif, 0.f, ocStr);


	/*//The x,z of the center of different chord types... ex: I, IV, V...
	float x1 = wx/2, z1=1900; //hard coding z for now
	float x2 = wx*23/32, z2=2200;
	float x3 = wx*11/16, z3=2600;
	float x4 = wx/2, z4=2800;
	float x5 = wx*5/16, z5=2600;
	float x6 = wx*9/32, z6=2200;

	float midX = wx/2, midZ = 2350; //best guess for z, for now*/

	//
	glColor3d(0.1,0.3,1.);
	float radius = 120;
	DrawCircle(winX*4/5, radius*3/2, radius, 18);

	glColor3d(1.,1.,1.);
	DrawRomNums(winX*4/5+10., radius*3/2, radius);

	float minOldX = (winX/2) - (29.25*winX/64); //le sigh
	float maxOldX = (winX/2) + (29.25*winX/64);
	//If the user location is within range, draw it on the circle. (as if looking down on user's head)
	if (avgLoc.z() >= 1675 && avgLoc.z() <= 3025 && avgLoc.x() >= minOldX && avgLoc.x() <= maxOldX)
	{
		float ztoy = 675/(radius*3/2); 
		float xtox = (winX/2)/(29.25*winX/64);
		float circleX = newScale(winX-3*radius, winX, minOldX+150., maxOldX, avgLoc.x() / xtox);
		float circleY = newScale(0., 3*radius, 1675/ztoy, 3025/ztoy, avgLoc.z() / ztoy);

		glColor3d(0.9, 0.0, 0.2);
		DrawRect(circleX - 50, circleY);
		//DrawCircle(circleX, circleY, 15., 10);
	}

	
	glColor3d(0., 0.8, 0.);
	glPolygonStipple(halftone);
	glBegin(GL_QUADS);
		glVertex3d(25., 300, 0.f);
		glVertex3d(25., 375, 0.f);
		glVertex3d(75., 375, 0.f);
		glVertex3d(75., 300, 0.f);
	glEnd();
	glDisable(GL_POLYGON_STIPPLE);

	glLineWidth(5.);
	glBegin(GL_LINE_LOOP);
		glVertex3d(25., 300, 0.f);
		glVertex3d(25., 375, 0.f);
		glVertex3d(75., 375, 0.f);
		glVertex3d(75., 300, 0.f);
	glEnd();
	

	glColor3d(0.8, 0., 0.);
	glPolygonStipple(halftone);
	glBegin(GL_QUADS);
		glVertex3d(25., 500, 0.f);
		glVertex3d(25., 575, 0.f);
		glVertex3d(75., 575, 0.f);
		glVertex3d(75., 500, 0.f);
	glEnd();
	glDisable(GL_POLYGON_STIPPLE);

	glLineWidth(5.);
	glBegin(GL_LINE_LOOP);
		glVertex3d(25., 500, 0.f);
		glVertex3d(25., 575, 0.f);
		glVertex3d(75., 575, 0.f);
		glVertex3d(75., 500, 0.f);
	glEnd();
}